#include "visualserver.h"

//#include <opencv2/highgui/highgui_c.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>


#define DEBUG 0
using namespace std;
using namespace cv;
void* transferDaemon(void* server){
    ((VisualServer*) server)->imageTransferDaemon();
    return (void*)0;
}

VisualServer::VisualServer()
{
    this->m_has_connection = false;
    this->m_tran_existing = false;
    sem_init(&this->m_transfer_mutex, 0, 1);
    sem_init(&this->m_transfer_addr_mutex, 0, 1);
    sem_init(&this->m_transfer_sync, 0, 0);
    pthread_create(&this->m_tran_thread, NULL, transferDaemon, (void*)this);
    this->m_ptz.initial();
    this->m_transfer.setRequestHandler(this);
    this->m_image_data_queue.setSize(100);
    this->m_tran_switch = false;
    this->m_transfer.setDataSwitch(this->m_tran_switch);
    this->m_tran_interval = 12;

}

VisualServer::~VisualServer(){
    this->m_tran_existing = true;
    while(!this->m_need_to_tran.empty()){
        this->m_need_to_tran.pop();
    }
    sem_post(&this->m_transfer_sync);
    pthread_cancel(this->m_tran_thread);

}
void VisualServer::packResponse(void *response){
    responseInfo(response)->timestamp = TimeStamp::now()->toInt();
    responseInfo(response)->magic = RESPONSE_MAGIC;
}

void VisualServer::sendImageBlock(unsigned int ts, vector<unsigned char>& data, int block, sockaddr_in _addr, socklen_t _addr_len){
    if(block == -1){
        int _count = (data.size() + RESPONSE_DATA_LEN - 1) / RESPONSE_DATA_LEN;
        for (int i = 0; i < _count; ++i)
        {
            response_data_t _data;
            unsigned int _len;
            int _remain = data.size() - i * RESPONSE_DATA_LEN;
            if(_remain > 0){
                _len = (unsigned int) _remain > RESPONSE_DATA_LEN ? RESPONSE_DATA_LEN : _remain;
                responseInfo(&_data)->type = IMAGE_RESPONSE;
                setResponseDataInfo(&_data, IMAGE_TIMESTAMP, ts);
                setResponseDataInfo(&_data, IMAGE_SIZE, (int)data.size());
                setResponseDataInfo(&_data, IMAGE_SEQ, i);
                setResponseDataInfo(&_data, IMAGE_SEQ_SIZE, _len);
                memcpy(responseDataPtr(&_data), &data[i * RESPONSE_DATA_LEN], _len);
                this->packResponse(&_data);
                this->m_transfer.sendData(&_data, sizeof(_data), _addr, _addr_len);
            }
        }
    }else if(block > 0){
        response_data_t _data;
        int _len;
        int id = block;
        int _remain = data.size() - id * RESPONSE_DATA_LEN;
        if(_remain > 0){
            _len = (unsigned int) _remain > RESPONSE_DATA_LEN ? RESPONSE_DATA_LEN : _remain;
            responseInfo(&_data)->type = IMAGE_RESPONSE;
            setResponseDataInfo(&_data, IMAGE_TIMESTAMP, ts);
            setResponseDataInfo(&_data, IMAGE_SIZE, (int)data.size());
            setResponseDataInfo(&_data, IMAGE_SEQ, id);
            setResponseDataInfo(&_data, IMAGE_SEQ_SIZE, _len);
            memcpy(responseDataPtr(&_data), &data[id * RESPONSE_DATA_LEN], _len);
            packResponse(&_data);
            this->m_transfer.sendData(&_data, sizeof(_data), _addr, _addr_len);
        }
    }
}

void VisualServer::imageTransferDaemon(){
    while(!this->m_tran_existing){
        sem_wait(&this->m_transfer_addr_mutex);
        struct sockaddr_in _addr = this->m_tran_addr;
        socklen_t _addr_len = this->m_tran_addr_len;
        sem_post(&this->m_transfer_addr_mutex);
        sem_wait(&this->m_transfer_sync);
        sem_wait(&this->m_transfer_mutex);
        if(!this->m_need_to_tran.empty()){
            image_block_ref_t block = this->m_need_to_tran.front();
            this->m_need_to_tran.pop();
            vector<unsigned char> data;
            if(this->m_image_data_queue.find(block.timestamp, &data)){
//                printf("in transfer daemon\n");fflush(stdout);
                if(this->m_has_connection)
                    sendImageBlock(block.timestamp, data, block.block, _addr, _addr_len);
            }
        }
        sem_post(&this->m_transfer_mutex);
    }
}
bool VisualServer::answerCameraRequest(normal_request_t request, normal_response_t &response){
    if(requestInfo(&request)->action == SET_GET){
        int focus = getRequestField(&request, CAMERA_FOCUS);
        int brightness = getRequestField(&request, CAMERA_BRIGHTNESS);
        int contrast = getRequestField(&request, CAMERA_CONTRAST);
        int hue = getRequestField(&request, CAMERA_HUE);
        int saturation = getRequestField(&request, CAMERA_SATURATION);
        int record = getRequestField(&request, CAMERA_RECORD);

        if(focus >=0)
            this->m_camera.setProp(CAMERA_FOCUS_ID, focus);
        if(brightness >= 0)
            this->m_camera.setProp(CAMERA_BRIGHTNESS_ID, (double)brightness / 100 );
        if(contrast >= 0)
            this->m_camera.setProp(CAMERA_CONTRAST_ID, (double)contrast / 100 );
        if(hue >= 0)
            this->m_camera.setProp(CAMERA_HUE_ID, (double)hue / 100);
        if(saturation >= 0)
            this->m_camera.setProp(CAMERA_SATURATION_ID, (double)saturation / 100);

        if(record >= 0){
            if(record & 1){
                this->m_camera.startRecord();
                printf("Camera: start recording!\n");
            }else{
                this->m_camera.stopRecord();
                printf("Camera: stop recording!\n");
            }
        }
#if DEBUG
        printf("camera rquest b =%d c = %d h = %d s = %d r = %d\n", brightness, contrast, hue, saturation, record);
#endif

    }
    int focus = (int)this->m_camera.getProp(CAMERA_FOCUS_ID);
    int brightness = (int)((this->m_camera.getProp(CAMERA_BRIGHTNESS_ID) + 0.005) * 100);
    int contrast = (int)((this->m_camera.getProp(CAMERA_CONTRAST_ID) + 0.005) * 100);
    int hue = (int)((this->m_camera.getProp(CAMERA_HUE_ID) + 0.005) * 100);
    int saturation = (int)((this->m_camera.getProp(CAMERA_SATURATION_ID) + 0.005) * 100);
    int record = this->m_camera.getRecordState()?1:0;
    responseInfo(&response)->type = CAMERA_RESPONSE;
    setResponseField(&response, CAMERA_FOCUS, focus);
    setResponseField(&response, CAMERA_BRIGHTNESS, brightness);
    setResponseField(&response, CAMERA_CONTRAST, contrast);
    setResponseField(&response, CAMERA_HUE, hue);
    setResponseField(&response, CAMERA_SATURATION,saturation);
    setResponseField(&response, CAMERA_RECORD, record);
    this->packResponse(&response);
#if DEBUG
        printf("camera response b =%d c = %d h = %d s = %d r = %d\n", brightness, contrast, hue, saturation, record );
#endif
}

bool VisualServer::answerImageRequest(normal_request_t request, normal_response_t &response){
    unsigned int ts = (unsigned int) getRequestField(&request, IMAGE_TIMESTAMP);
    int seq = getRequestField(&request, IMAGE_SEQ);
    int tran_switch = getRequestField(&request, IMAGE_TRAN_SWITCH);
    int speed = getRequestField(&request, IMAGE_TRAN_SPEED);
    if(speed >0){

        this->m_tran_interval = VISUAL_SERVER_INTERVAL / speed ;
    }
    if(ts != (unsigned int) -1){
        sem_wait(&this->m_transfer_mutex);
        if(this->m_image_data_queue.find(ts, NULL)){
            image_block_ref_t ref;
            ref.timestamp = ts;
            ref.block = seq;
            this->m_need_to_tran.push(ref);
        }
        sem_post(&this->m_transfer_mutex);
        sem_post(&this->m_transfer_sync);
    }
    if(tran_switch >= 0){
        if(tran_switch & 1){
            this->m_tran_switch = true;
        }else{
            this->m_tran_switch = false;
        }
        this->m_transfer.setDataSwitch(m_tran_switch);
    }

#if DEBUG
        printf("image request ts = %u seq = %d speed = %d switch = %d\n", ts, seq, speed, tran_switch);
#endif
    return true;
}

bool VisualServer::answerPtzRequest(normal_request_t request, normal_response_t &response){
    int x, y, z;
     if(requestInfo(&request)->action == SET_GET){
         x = getRequestField(&request, PTZ_X);
         y = getRequestField(&request, PTZ_Y);
         z = getRequestField(&request, PTZ_Z);
         this->m_ptz.set(x, y, z);
#if DEBUG
         printf("ptz request x = %d y = %d z = %d\n", x, y, z);
#endif
     }
     this->m_ptz.get(x, y, z);
     responseInfo(&response)->type = PTZ_RESPONSE;
     setResponseField(&response, PTZ_X, x);
     setResponseField(&response, PTZ_Y, y);
     setResponseField(&response, PTZ_Z, z);
     this->packResponse(&response);
     return true;
}

bool VisualServer::answerSyncRequest(normal_request_t request, normal_response_t &response){
    int time, count, data_switch, info_switch;
    if(requestInfo(&request)->action == SET_GET){
        data_switch = getRequestField(&request, SYNC_DATA_SWITCH);
        info_switch = getRequestField(&request, SYNC_INFO_SWITCH);
        this->m_transfer.setDataSwitch(data_switch == 0?false:true);
        this->m_transfer.setInfoSwitch(info_switch == 0?false:true);
    }
    time = getRequestField(&request, SYNC_TIME);
    count = getRequestField(&request, SYNC_COUNT);

    responseInfo(&response)->type = SYNC_RESPONSE;
    data_switch = this->m_transfer.getDataSwitch()?1:0;
    info_switch = this->m_transfer.getInfoSwitch()?1:0;
    setResponseField(&response, SYNC_TIME, time);
    setResponseField(&response, SYNC_COUNT, count);
    setResponseField(&response, SYNC_DATA_SWITCH, data_switch);
    setResponseField(&response, SYNC_INFO_SWITCH, info_switch);
    this->packResponse(&response);
    return true;
}

void VisualServer::infoHandler(normal_request_t request, sockaddr_in addr, socklen_t addr_len){
    normal_response_t response;
    memset(&response, 0, sizeof(response));
    switch(requestInfo(&request)->type){
    case IMAGE_REQUEST:
        answerImageRequest(request, response);
        break;
    case PTZ_REQUEST:
        answerPtzRequest(request, response);
        break;
    case CAMERA_REQUEST:
        answerCameraRequest(request, response);
        break;
    }
    if(responseInfo(&response)->type != 0){
        this->m_transfer.sendInfo(&response, sizeof(response), addr, addr_len);
    }
}

void VisualServer::syncHandler(normal_request_t request, sockaddr_in addr, socklen_t addr_len){
    normal_response_t response;
    this->answerSyncRequest(request, response);
    this->m_transfer.sendSync(&response, sizeof(response), addr, addr_len);
    static sockaddr_in _addr;
    if(_addr.sin_addr.s_addr != addr.sin_addr.s_addr){
        _addr = addr;
        sem_wait(&this->m_transfer_addr_mutex);
        this->m_tran_addr = addr;
        this->m_tran_addr.sin_port = htons(DEFAULT_CLIENT_INFO_PORT);
        this->m_tran_addr_len = addr_len;
        this->m_has_connection = true;
        sem_post(&this->m_transfer_addr_mutex);
    }
}

void VisualServer::dataHandler(normal_request_t request, sockaddr_in addr, socklen_t addr_len){

}

void VisualServer::sendPtzInfo(int interval){
    static int count = 0;
    if(count >= interval){
        count = 0;
        if(!this->m_has_connection)
            return;
        int x, y, z;
        this->m_ptz.get(x, y, z);
        normal_response_t response;
        responseInfo(&response)->type = PTZ_RESPONSE;
        setResponseField(&response, PTZ_X, x);
        setResponseField(&response, PTZ_Y, y);
        setResponseField(&response, PTZ_Z, z);
        this->packResponse(&response);
        sockaddr_in addr;
        socklen_t addr_len;
        sem_wait(&this->m_transfer_addr_mutex);
        addr = this->m_tran_addr;
        addr_len = this->m_tran_addr_len;
        sem_post(&this->m_transfer_addr_mutex);
        this->m_transfer.sendInfo(&response, sizeof(response), addr, addr_len);
    }
    count ++;
}
void VisualServer::sendCameraInfo(int interval){
    static int count = 0;
    if(count >= interval){
        count = 0;
        if(!this->m_has_connection)
            return;
        normal_response_t response;
        int focus = (int)this->m_camera.getProp(CAMERA_FOCUS_ID);
        int brightness = (int)((this->m_camera.getProp(CAMERA_BRIGHTNESS_ID) + 0.005) * 100);
        int contrast = (int)((this->m_camera.getProp(CAMERA_CONTRAST_ID) + 0.005) * 100);
        int hue = (int)((this->m_camera.getProp(CAMERA_HUE_ID) + 0.005) * 100);
        int saturation = (int)((this->m_camera.getProp(CAMERA_SATURATION_ID) + 0.005) * 100);
        int record = this->m_camera.getRecordState()?1:0;
        responseInfo(&response)->type = CAMERA_RESPONSE;
        setResponseField(&response, CAMERA_FOCUS, focus);
        setResponseField(&response, CAMERA_BRIGHTNESS, brightness);
        setResponseField(&response, CAMERA_CONTRAST, contrast);
        setResponseField(&response, CAMERA_HUE, hue);
        setResponseField(&response, CAMERA_SATURATION,saturation);
        setResponseField(&response, CAMERA_RECORD, record);
        this->packResponse(&response);
        sockaddr_in addr;
        socklen_t addr_len;
        sem_wait(&this->m_transfer_addr_mutex);
        addr = this->m_tran_addr;
        addr_len = this->m_tran_addr_len;
        sem_post(&this->m_transfer_addr_mutex);
        this->m_transfer.sendInfo(&response, sizeof(response), addr, addr_len);
    }
    count ++;
}

void VisualServer::sendImageInfo(Mat& img, unsigned int ts, int interval){
    static int count = 0;
    if(count >= interval){
        count = 0;
        if(!this->m_has_connection)
            return;
        normal_response_t response;
        vector<unsigned char> data = vector<unsigned char>();
        vector<int> param;
        param.push_back(CV_IMWRITE_JPEG_QUALITY);
        param.push_back(100);
        imencode(".jpg", img, data);
        sem_wait(&this->m_transfer_mutex);
        this->m_image_data_queue.enqueue(ts, data);
        sem_post(&this->m_transfer_mutex);
        responseInfo(&response)->type = IMAGE_RESPONSE;
        setResponseField(&response, IMAGE_TIMESTAMP, (int)ts);
        int total_seq = (data.size() + RESPONSE_DATA_LEN -1) /  RESPONSE_DATA_LEN;
        setResponseField(&response, IMAGE_SEQ, total_seq);
        setResponseField(&response, IMAGE_WIDTH, img.cols);
        setResponseField(&response, IMAGE_HEIGHT, img.rows);
        setResponseField(&response, IMAGE_SIZE, data.size());
        setResponseField(&response, IMAGE_TRAN_SWITCH, this->m_tran_switch? 1: 0);
        setResponseField(&response, IMAGE_TRAN_SPEED, VISUAL_SERVER_INTERVAL / this->m_tran_interval);
        this->packResponse(&response);
        sockaddr_in addr;
        socklen_t addr_len;
        sem_wait(&this->m_transfer_addr_mutex);
        addr = this->m_tran_addr;
        addr_len = this->m_tran_addr_len;
        sem_post(&this->m_transfer_addr_mutex);
        this->m_transfer.sendInfo(&response, sizeof(response), addr, addr_len);
    }
    ++count;
}

//for run
const char* g_keys =
{
    "{c| camera |-1   | camera number}"
    "{f| file   |false| use file as source}"
    "{np|nopreview| false| without preview}"
    "{1|        |nofile   |source file}"
    "{w|wait    |-1   |go straight to the frame}"
    "{b|begin   | 0    |begin frame}"
    "{s|save    |false| save video}"
};
void VisualServer::run(int argc, const char **argv){
    //parse command line
    CommandLineParser parser(argc, argv, g_keys);
    int device = parser.get<int>("camera");
    bool use_file = parser.get<bool>("file");
    string file_name = parser.get<string>("1");
    bool no_preview = parser.get<bool>("nopreview");
    bool save_video = parser.get<bool>("save");

    printf("device = %d\n", device);
    if(use_file){
        printf("filename = %s\n", file_name.c_str());
    }else{
        printf("usefile = false\n");
    }
    if(no_preview){
        printf("nopreview = true\n");
    }else{
        printf("nopreview = false\n") ;
    }

    //initial

    if(use_file){
        this->m_camera.startCapture(file_name, 30);
    }else{
        this->m_camera.startCapture(device, 24);

    }
    if(save_video){
        this->m_camera.startRecord();
    }

    this->m_transfer.startAnswer();

    // main routine
    if(!no_preview)
        namedWindow("preview", 1);
    Mat img;
    TimeStamp ts;
    TimeStamp img_ts;
    for(;;){
        if(this->m_camera.getImage(img, img_ts)){
            if(!no_preview)
                imshow("preview", img);
        }
        this->sendPtzInfo();
        this->sendCameraInfo();
        this->sendImageInfo(img, ts.toInt(), this->m_tran_interval);
        int cost = TimeStamp::now()->diff(ts);
        int interval;
        if( cost < VISUAL_SERVER_INTERVAL && cost >= 0 ){
           interval = VISUAL_SERVER_INTERVAL - cost;
        }else{
            interval = 1;
        }
        ts.setNow();
        if(waitKey(interval) > 0)
            break;
    }
}
